
import os
import yaml

import ament_index_python.packages
import launch
import launch_ros.actions


def generate_launch_description():
    driver_share_dir = ament_index_python.packages.get_package_share_directory('lidar209_driver')
    driver_params_file = os.path.join(driver_share_dir, 'config', 'driver_node_params.yaml')
    lidar209_driver_node = launch_ros.actions.Node(package='lidar209_driver',
                                                   executable='lidar209_driver_node',
                                                   output='both',
                                                   parameters=[driver_params_file])

    convert_share_dir = ament_index_python.packages.get_package_share_directory('to_pointcloud')
    convert_params_file = os.path.join(convert_share_dir, 'config', 'qs176_cloud_params.yaml')
    with open(convert_params_file, 'r') as f:
        convert_params = yaml.safe_load(f)['to_pointcloud_node']['ros__parameters']
    convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'CZLidar_QS176_ZW3_36.yaml')
    to_pointcloud_node = launch_ros.actions.Node(package='to_pointcloud',
                                                    executable='to_pointcloud_node',
                                                    output='both',
                                                    parameters=[convert_params])

    return launch.LaunchDescription([lidar209_driver_node,
                                     to_pointcloud_node,

                                     launch.actions.RegisterEventHandler(
                                         event_handler=launch.event_handlers.OnProcessExit(
                                             target_action=lidar209_driver_node,
                                             on_exit=[launch.actions.EmitEvent(
                                                 event=launch.events.Shutdown())],
                                         )),
                                     ])
